{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "  COLAB = True\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  COLAB = False\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 4.0 Numerical Inverse Kinematics\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[4.1 Numerical Inverse Kinematics Utility Class](#utility)\n",
    "* Resolved-Rate Motion Control\n",
    "  * Angle-axis error component\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[4.2 Newton-Raphson (NR) Method](#nr)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[4.2 Newton-Raphson (NR) Method](#nr)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[4.3 Gauss-Newton (GN) Method](#gn)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[4.4 Levenberg-Marquardt (LM) Method](#lm)\n",
    "* Numerical Inverse Kinematics\n",
    "\n",
    "[4.5 Visualise IK Solutions](#vis)\n",
    "\n",
    "[4.6 Numerical IK Comparison](#exp)\n",
    "\n",
    "[4.7 Fast IK Solvers with the Robotics Toolbox](#rtb)\n",
    "\n",
    "[4.8 Robotics Toolbox IK Speed Comparison](#speed)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "\n",
    "# ansitable is a great package for printing tables in a terminal\n",
    "from ansitable import ANSITable\n",
    "\n",
    "# python mechanisms to create abstract classes\n",
    "from abc import ABC, abstractmethod\n",
    "\n",
    "# a package for creating dynamic progress bars\n",
    "from progress.bar import Bar\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes with the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# provides sleep functionaly\n",
    "import time\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='utility'></a>\n",
    "\n",
    "### 4.1 Numerical Inverse Kinematics Utility Class\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inverse kinematics deals is the problem of determining the corresponding joint coordinates, given some end-effector pose. Numerical inverse kinematics as opposed to analytical inverse kinematics uses an iterative technique.\n",
    "\n",
    "We will start this Notebook by making an abstract base class called `IK`. This class implements everything needed to support a numerical inverse kinematics solver. It stores some general parameters typically used by any IK solver and also has some data structures useful for large-scale experiments. The idea behind the `IK` class is that a specific method class will inherit `IK` and implement the `step` method."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An important point to note is that numerical IK methods are subject to local minima and in such some will fail to converge on the solution. The choice of the initial joint configuration $\\bf{q}_0$ is important.\n",
    "\n",
    "An alternative approach is to re-start an IK problem with a new random $\\bf{q}_0$ after a few $20 \\sim 50$ iterations rather than persist with a single search attempt with $500 \\sim 5000$ iterations. This is a simple but effective method of performing a global search for the IK solution. We can adjust this using the iteration limit `ilimit` and search limit `slimit` kwargs on the `IK` class."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the Tutorial, we use a quadratic error term to describe the error between the current end-effector pose and desired end-effector pose. This error is consistent across each method we discuss.\n",
    "\n",
    "Each method seeks to minimise the error function\n",
    "\n",
    "\\begin{align*}\n",
    "    E = \\frac{1}{2} \\bf{e}^{\\top} \\bf{W}_e \\bf{e}\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{e} \\in \\mathbb{R}^6$ is the position and angle-axis error vector (see Notebook 3), and $\\bf{W}_e = \\text{diag}(\\bf{w_e})(\\bf{w_e} \\in \\mathbb{R}^n_{\\geq0})$ is a diagonal weighting matrix which prioritises the corresponding error term. The `IK` class below takes a `we` vector in the `init` method and calculates the error vector `e` and quadratic error term `E` through the `error` method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IK(ABC):\n",
    "    \"\"\"\n",
    "    An abstract super class which provides basic functionality to perform numerical inverse\n",
    "    kinematics (IK). Superclasses can inherit this class and implement the solve method.\n",
    "\n",
    "    This class also provides a mechanism to collect data on performance for large scale\n",
    "    experiments.\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(\n",
    "        self,\n",
    "        name: str = \"IK Solver\",\n",
    "        ilimit: int = 30,\n",
    "        slimit: int = 100,\n",
    "        tol: float = 1e-6,\n",
    "        we: np.ndarray = np.ones(6),\n",
    "        problems: int = 1000,\n",
    "    ):\n",
    "        \"\"\"\n",
    "        name: The name of the IK algorithm\n",
    "        ilimit: How many iterations are allowed within a search before a new search is started\n",
    "        slimit: How many searches are allowed before being deemed unsuccessful\n",
    "        tol: Maximum allowed residual error E\n",
    "        we: A 6 vector which assigns weights to Cartesian degrees-of-freedom\n",
    "        problems: Total number of IK problems within the experiment\n",
    "        \"\"\"\n",
    "\n",
    "        # Solver parameters\n",
    "        self.name = name\n",
    "        self.slimit = slimit\n",
    "        self.ilimit = ilimit\n",
    "        self.tol = tol\n",
    "        self.We = np.diag(we)\n",
    "\n",
    "        # Solver results\n",
    "        self.success = np.zeros(problems)\n",
    "        self.searches = np.zeros(problems)\n",
    "        self.iterations = np.zeros(problems)\n",
    "\n",
    "        # initialise with NaN\n",
    "        self.searches[:] = np.nan\n",
    "        self.iterations[:] = np.nan\n",
    "        self.success[:] = np.nan\n",
    "\n",
    "    def solve(self, ets: rtb.ETS, Tep: np.ndarray, q0: np.ndarray):\n",
    "        \"\"\"\n",
    "        This method will attempt to solve the IK problem and obtain joint coordinates\n",
    "        which result the the end-effector pose Tep.\n",
    "\n",
    "        The method returns a tuple:\n",
    "        q: The joint coordinates of the solution (ndarray). Note that these will not\n",
    "            be valid if failed to find a solution\n",
    "        success: True if a solution was found (boolean)\n",
    "        iterations: The number of iterations it took to find the solution (int)\n",
    "        searches: The number of searches it took to find the solution (int)\n",
    "        residual: The residual error of the solution (float)\n",
    "        \"\"\"\n",
    "\n",
    "        # Iteration count\n",
    "        i = 0\n",
    "        total_i = 0\n",
    "\n",
    "        for search in range(self.slimit):\n",
    "            q = q0[search].copy()\n",
    "            \n",
    "            while i <= self.ilimit:\n",
    "                i += 1\n",
    "\n",
    "                # Attempt a step\n",
    "                # try:\n",
    "                E, q = self.step(ets, Tep, q)\n",
    "                # except np.linalg.LinAlgError:\n",
    "                #     i = np.nan\n",
    "                #     break\n",
    "\n",
    "                # Check if we have arrived\n",
    "                if E < self.tol:\n",
    "                    return q, True, total_i + i, search + 1, E\n",
    "\n",
    "            total_i += i\n",
    "            i = 0\n",
    "\n",
    "        # If we make it here, then we have failed\n",
    "        return q, False, np.nan, np.nan, E\n",
    "\n",
    "    def error(self, Te: np.ndarray, Tep: np.ndarray):\n",
    "        \"\"\"\n",
    "        Calculates the engle axis error between current end-effector pose Te and\n",
    "        the desired end-effector pose Tep. Also calulates the quadratic error E\n",
    "        which is weighted by the diagonal matrix We.\n",
    "\n",
    "        Returns a tuple:\n",
    "        e: angle-axis error (ndarray in R^6)\n",
    "        E: The quadratic error weighted by We\n",
    "        \"\"\"\n",
    "        e = rtb.angle_axis(Te, Tep)\n",
    "        E = 0.5 * e @ self.We @ e\n",
    "\n",
    "        return e, E\n",
    "\n",
    "    @abstractmethod\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        \"\"\"\n",
    "        Superclasses will implement this method to perform a step of the implemented\n",
    "        IK algorithm\n",
    "        \"\"\"\n",
    "        pass\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='nr'></a>\n",
    "\n",
    "### 4.2 Newton-Raphson (NR) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Newton-Raphson (NR) method seeks to minimise the error $E$ by iterating upon the following\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} = \\bf{q}_k + {^0\\bf{J}(\\bf{q}_k)}^{-1} \\bf{e}_k.\n",
    "\\end{align*}\n",
    "\n",
    "When using the NR method, the initial joint coordinates $q_0$, should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.\n",
    "When the the problem is solvable, it converges very quickly.\n",
    "However, this method frequently fails to converge on the goal.\n",
    "\n",
    "Most linear algebra libraries (including the Python `numpy` library) implement the pseudoinverse using singular value decomposition, which is robust to singular matrices. Therefore, we can this solver more robust by using the pseudoinverse instead of the normal inverse. \n",
    "\n",
    "The above equation requires the Jacobian to be square and non-singular. For a redundant manipulator (>6 DoF) or for more robust results to singularities we can use the pseudoinverse. We can use the pseudoinverse by supplying `pinv=True` when initialising the `NR` class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "class NR(IK):\n",
    "    def __init__(self, pinv=False, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        self.pinv = pinv\n",
    "\n",
    "        self.name = f\"NR (pinv={pinv})\"\n",
    "\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        J = ets.jacob0(q)\n",
    "\n",
    "        if self.pinv:\n",
    "            q += np.linalg.pinv(J) @ e\n",
    "        else:\n",
    "            q += np.linalg.inv(J) @ e\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='gn'></a>\n",
    "\n",
    "### 4.3 Gauss-Newton (GN) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can improve the solvability of the NR method by using the Gauss-Newton (GN) method\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} &= \\bf{q}_k +\n",
    "    \\left(\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e \\\n",
    "    {\\bf{J}(\\bf{q}_k)}\n",
    "    \\right)^{-1}\n",
    "    \\bf{g}_k \\\\\n",
    "    \\bf{g}_k &=\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e\n",
    "    \\bf{e}_k\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{J} = {^0\\bf{J}}$ is the base-frame manipulator Jacobian. If $\\bf{J}(\\bf{q}_k)$ is non-singular, and $\\bf{W}_e = \\bf{1}_n$, then the above provides the pseudoinverse solution. However, if $\\bf{J}(\\bf{q}_k)$ is singular, the above can not be computed and the GN solution is infeasible.\n",
    "\n",
    "Most linear algebra libraries (including the Python `numpy` library) implement the pseudoinverse using singular value decomposition, which is robust to singular matrices. Therefore, we can this solver more robust by using the pseudoinverse instead of the normal inverse. We can use the pseudoinverse by supplying `pinv=True` when initialising the `GN` class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "class GN(IK):\n",
    "    def __init__(self, pinv=False, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        self.pinv = pinv\n",
    "\n",
    "        self.name = f\"GN (pinv={pinv})\"\n",
    "\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        if self.pinv:\n",
    "            q += np.linalg.pinv(J.T @ self.We @ J) @ g\n",
    "        else:\n",
    "            q += np.linalg.inv(J.T @ self.We @ J) @ g\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='lm'></a>\n",
    "\n",
    "### 4.4 Levenberg-Marquardt (LM) Method\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can further improve the solvability though the Levenberg-Marquardt (LM) Method\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{q}_{k+1} \n",
    "    &= \n",
    "    \\bf{q}_k +\n",
    "    \\left(\n",
    "        \\bf{A}_k\n",
    "    \\right)^{-1}\n",
    "    \\bf{g}_k \\\\\n",
    "    %\n",
    "    \\bf{A}_k\n",
    "    &=\n",
    "    {\\bf{J}(\\bf{q}_k)}^\\top\n",
    "    \\bf{W}_e \\\n",
    "    {\\bf{J}(\\bf{q}_k)}\n",
    "    +\n",
    "    \\bf{W}_n\n",
    "\\end{align*}\n",
    "\n",
    "where $\\bf{W}_n = \\text{diag}(\\bf{w_n})(\\bf{w_n} \\in \\mathbb{R}^n_{>0})$ is a diagonal damping matrix. The damping matrix ensures that $\\bf{A}_k$ is non-singular and positive definite. The performance of the LM method largely depends on the choice of $\\bf{W}_n$."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Wampler's Method\n",
    "\n",
    "Wampler proposed $\\bf{w_n}$ to be a constant. We provide the variable `λ` as a kwarg to the `LM_Wampler` class to adjust the constant."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Wampler(IK):\n",
    "    def __init__(self, λ=1.0, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        \n",
    "        self.name = f\"LM (Wampler λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = self.λ * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Chan's Method\n",
    "\n",
    "Chan proposed\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{W}_n\n",
    "    &=\n",
    "    \\lambda E_k \\bf{1}_n\n",
    "\\end{align*}\n",
    "\n",
    "where $\\lambda$ is a constant which reportedly does not have much influence on performance. We provide the variable `λ` as a kwarg to the `LM_Chan` class to adjust the weighting term."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Chan(IK):\n",
    "    def __init__(self, λ=1.0, **kwargs):\n",
    "        super().__init__(**kwargs)\n",
    "        \n",
    "        self.name = f\"LM (Chan λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = self.λ * E * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g\n",
    "\n",
    "        return E, q\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Sugihara's Method\n",
    "\n",
    "Sugihara proposed\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{W}_n\n",
    "    &=\n",
    "    E_k \\bf{1}_n + \\text{diag}(\\hat{\\bf{w}}_n)\n",
    "\\end{align*}\n",
    "\n",
    "where $\\hat{\\bf{w}}_n \\in \\mathbb{R}^n$, $\\hat{w}_{n_i} = l^2 \\sim 0.01 l^2$, and $l$ is the length of a typical link within the manipulator.\n",
    "We provide the variable `λ` as a kwarg to the `LM_Sugihara` class to adjust the value of $w_n$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "class LM_Sugihara(IK):\n",
    "    def __init__(self, name=\"LM (Sugihara)\", λ=1.0, **kwargs):\n",
    "        super().__init__(name, **kwargs)\n",
    "\n",
    "        self.name = f\"LM (Sugihara λ={λ})\"\n",
    "        self.λ = λ\n",
    "\n",
    "    def step(self, ets: rtb.ETS, Tep: np.ndarray, q: np.ndarray):\n",
    "        Te = ets.eval(q)\n",
    "        e, E = self.error(Te, Tep)\n",
    "\n",
    "        Wn = E * np.eye(ets.n) + self.λ * np.eye(ets.n)\n",
    "        J = ets.jacob0(q)\n",
    "        g = J.T @ self.We @ e\n",
    "\n",
    "        q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g\n",
    "\n",
    "        return E, q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='vis'></a>\n",
    "\n",
    "### 4.5 Visualise IK Solutions\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: This **section** of the notebook must be run locally. Due to how Swift operates, this section will not run on Google Colab_\n",
    "\n",
    "We are going to try out some of our methods and visualise the results in Swift"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Make a panda robot\n",
    "panda = rtb.models.Panda()\n",
    "ets = panda.ets()\n",
    "\n",
    "# Our IK methods do not respect joint limits. Therefore, many solutions\n",
    "# are likely to be in violation of the joint limits. Swift enforces joint\n",
    "# limits when stepping or simulating the environment. To properly\n",
    "# visualise our results, we must remove the joint limits of the robot.\n",
    "for link in panda.links:\n",
    "    if link.isjoint:\n",
    "        # link.qlim = None\n",
    "        link.qlim = [-np.inf, np.inf]\n",
    "        # link.qlim = [np.pi, -np.pi]\n",
    "\n",
    "# Make an axes to visualise our current end-effector pose\n",
    "ee_axes = sg.Axes(0.1)\n",
    "\n",
    "# Make an axes to visualise our desired end-effector pose\n",
    "goal_axes = sg.Axes(0.1)\n",
    "\n",
    "def make_ik_env():\n",
    "    # Make the environment\n",
    "    env = Swift()\n",
    "\n",
    "    # Launch the simulator\n",
    "    env.launch(realtime=True, browser=\"notebook\")\n",
    "\n",
    "    # Add our robot to the simulator envionment\n",
    "    env.add(panda)\n",
    "\n",
    "    # Add axes to the environment\n",
    "    env.add(ee_axes)\n",
    "    env.add(goal_axes)\n",
    "\n",
    "    return env"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will establish some goal poses and a general visualise method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems\n",
    "problems = 10\n",
    "\n",
    "# Iteration and seach limits\n",
    "ilimit = 30\n",
    "slimit = 100\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(slimit)\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "def visualise_ik(solver, env):\n",
    "    \"\"\"\n",
    "    A method which will test the solver over each pose in Tep\n",
    "    and visualise the results in Swift\n",
    "    \"\"\"\n",
    "\n",
    "    for pose in Tep:\n",
    "\n",
    "        # Solver for Tep\n",
    "        q, success, iterations, searches, residual = solver.solve(ets, pose, q0)\n",
    "\n",
    "        # q = np.unwrap(q, period=np.pi)\n",
    "\n",
    "        # print the results\n",
    "        print(f\"Successful: {success}, iterations: {iterations}, searches: {searches}, residual: {residual}\")\n",
    "\n",
    "        ### visualise the results\n",
    "        # set the pandas joint coordinates to the solution q\n",
    "        panda.q = q\n",
    "\n",
    "        # set the goal axes to Tep pose\n",
    "        goal_axes.T = pose\n",
    "\n",
    "        # set the end-effector axes to the end-effector pose at the solution q\n",
    "        ee_axes.T = panda.fkine(q)\n",
    "\n",
    "        # step the environment to view the changes\n",
    "        env.step(0)\n",
    "\n",
    "        # sleep for 5 seconds to view the results\n",
    "        time.sleep(2)\n",
    "\n",
    "       "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test out the NR method first. Since we are using the Panda robot which has 7 DoF and is redundant we must set `pinv=True`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52008/?53008\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x106a9e350>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Successful: True, iterations: 10, searches: 1, residual: 7.022203158071692e-10\n",
      "Successful: True, iterations: 41, searches: 2, residual: 2.53082645377669e-10\n",
      "Successful: True, iterations: 39, searches: 2, residual: 3.977989059999839e-09\n",
      "Successful: True, iterations: 110, searches: 4, residual: 3.4134450222529105e-08\n",
      "Successful: True, iterations: 21, searches: 1, residual: 1.0428848837030995e-07\n",
      "Successful: True, iterations: 7, searches: 1, residual: 1.7159403280498059e-09\n",
      "Successful: True, iterations: 7, searches: 1, residual: 6.690750029224123e-09\n",
      "Successful: True, iterations: 60, searches: 2, residual: 4.069758436907444e-12\n",
      "Successful: True, iterations: 55, searches: 2, residual: 6.184209844935089e-08\n",
      "Successful: True, iterations: 10, searches: 1, residual: 1.3392322250102707e-10\n"
     ]
    }
   ],
   "source": [
    "if not COLAB:\n",
    "    # Make a new Swift environment\n",
    "    env = make_ik_env()\n",
    "\n",
    "    # Make our solver\n",
    "    solver = NR(pinv=True, ilimit=30, slimit=100, problems=problems)\n",
    "\n",
    "    # Visualise the solver\n",
    "    visualise_ik(solver, env)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Visualise the LM Chan method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52009/?53009\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x14f76bb50>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Successful: True, iterations: 8, searches: 1, residual: 4.2199677955175596e-10\n",
      "Successful: True, iterations: 7, searches: 1, residual: 3.7548326236152557e-07\n",
      "Successful: True, iterations: 19, searches: 1, residual: 5.20721220230351e-07\n",
      "Successful: True, iterations: 6, searches: 1, residual: 5.651632338921346e-11\n",
      "Successful: True, iterations: 9, searches: 1, residual: 7.839074005121057e-12\n",
      "Successful: True, iterations: 7, searches: 1, residual: 1.6034262113909875e-08\n",
      "Successful: True, iterations: 7, searches: 1, residual: 6.223525967592386e-08\n",
      "Successful: True, iterations: 6, searches: 1, residual: 2.559148910056854e-07\n",
      "Successful: True, iterations: 5, searches: 1, residual: 2.0211585009308673e-08\n",
      "Successful: True, iterations: 9, searches: 1, residual: 3.605649429973e-10\n"
     ]
    }
   ],
   "source": [
    "if not COLAB:\n",
    "    # Make a new Swift environment\n",
    "    env = make_ik_env()\n",
    "\n",
    "    # Make our solver\n",
    "    solver = LM_Chan(λ=0.1, ilimit=30, slimit=100, problems=problems)\n",
    "\n",
    "    # Visualise the solver\n",
    "    visualise_ik(solver, env)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Feel free to modify the above cells to try out any of the other methods. You may have noticed that many of the solutions violated the joint limits of the panda. In Part II of the Tutorial we will address this issue."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='exp'></a>\n",
    "\n",
    "### 4.6 Numerical IK Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this Section we will perform a large scale experiment to compare the performance of each IK method.\n",
    "\n",
    "This Notebook is intended to be set up as an IK playground. Feel free to modify, extend, or create your own methods as shown above. Then, simply add your solver to the `solvers` list below and text how it compares to the existing methods. Also feel free to try out different robots!"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As shown above, the `ETS` class has its own method to generate random valid joint coordinate vectors. However, to make the table in the paper reporoducible, we replicate the method here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def random_q(ets: rtb.ETS, i: int = 1) -> np.ndarray:\n",
    "    \"\"\"\n",
    "    Generate a random valid joint configuration\n",
    "\n",
    "    :param i: number of configurations to generate\n",
    "\n",
    "    Generates a random q vector within the joint limits defined by\n",
    "    `ets.qlim`.\n",
    "    \"\"\"\n",
    "\n",
    "    if i == 1:\n",
    "        q = np.zeros(ets.n)\n",
    "\n",
    "        for i in range(ets.n):\n",
    "            q[i] = np.random.uniform(ets.qlim[0, i], ets.qlim[1, i])\n",
    "\n",
    "    else:\n",
    "        q = np.zeros((i, ets.n))\n",
    "\n",
    "        for j in range(i):\n",
    "            for i in range(ets.n):\n",
    "                q[j, i] = np.random.uniform(ets.qlim[0, i], ets.qlim[1, i])\n",
    "\n",
    "    return q"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What is shown below will recreate the data used for Table 1 in the Paper. It will take a while to run. If you want results faster, reduce the number `problems`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.UR5()\n",
    "ets = robot.ets()\n",
    "\n",
    "# Setting the seed to 0 will make it possible to regenrate the table\n",
    "# in the paper (provided no ther values were changed).\n",
    "np.random.seed(0)\n",
    "\n",
    "### Experiment parameters\n",
    "# Number of problems to solve\n",
    "problems = 10000\n",
    "\n",
    "# Maximum slimit, we will preallocate the random q0 values so each\n",
    "# solver uses the same set of random q0 values\n",
    "max_slimit = 100\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_Tep = ets.random_q(problems)\n",
    "\n",
    "# random valid q values for q0\n",
    "q0 = ets.random_q(max_slimit)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_Tep[i])\n",
    "\n",
    "solvers = [\n",
    "    #### Local Search\n",
    "    NR(ilimit=500, slimit=1, problems=problems),\n",
    "    GN(ilimit=500, slimit=1, problems=problems),\n",
    "    NR(pinv=True, ilimit=500, slimit=1, problems=problems),\n",
    "    GN(pinv=True, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Wampler(λ=1e-4, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Wampler(λ=1e-6, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Chan(λ=1.0, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Chan(λ=0.1, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Sugihara(λ=0.001, ilimit=500, slimit=1, problems=problems),\n",
    "    LM_Sugihara(λ=0.0001, ilimit=500, slimit=1, problems=problems),\n",
    "    #### Global Search\n",
    "    NR(problems=problems),\n",
    "    GN(problems=problems),\n",
    "    NR(pinv=True, problems=problems),\n",
    "    GN(pinv=True, problems=problems),\n",
    "    LM_Wampler(λ=1e-4, problems=problems),\n",
    "    LM_Wampler(λ=1e-6, problems=problems),\n",
    "    LM_Chan(λ=1.0, problems=problems),\n",
    "    LM_Chan(λ=0.1, problems=problems),\n",
    "    LM_Sugihara(λ=0.001, problems=problems),\n",
    "    LM_Sugihara(λ=0.0001, problems=problems),\n",
    "]\n",
    "\n",
    "with Bar(message=\"Prog\", max=problems, check_tty=False, hide_cursor=False) as bar:\n",
    "    for i, pose in enumerate(Tep):\n",
    "\n",
    "        for solver in solvers:\n",
    "            (\n",
    "                _,\n",
    "                solver.success[i],\n",
    "                solver.iterations[i],\n",
    "                solver.searches[i],\n",
    "                _,\n",
    "            ) = solver.solve(ets, pose, q0)\n",
    "\n",
    "        bar.next()\n",
    "    bar.finish()\n",
    "\n",
    "\n",
    "# Make a table to visualise the results\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Compared over {problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"sLimit\",\n",
    "    \"iLimit\",\n",
    "    \"Mean Iterations\",\n",
    "    \"Median Iterations\",\n",
    "    \"Infeasible\",\n",
    "    \"Infeasible %\",\n",
    "    \"Mean Searches\",\n",
    "    \"Max Searches\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for solver in solvers:\n",
    "    table.row(\n",
    "        solver.name,\n",
    "        solver.slimit,\n",
    "        solver.ilimit,\n",
    "        np.round(np.nanmean(solver.iterations), 2),\n",
    "        np.nanmedian(solver.iterations),\n",
    "        np.sum(np.isnan(solver.iterations)),\n",
    "        np.round(np.sum(np.isnan(solver.iterations)) / problems * 100.0, 2),\n",
    "        np.round(np.nanmean(solver.searches), 2),\n",
    "        np.nanmax(solver.searches),\n",
    "    )\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "┌───────────────────────┬────────┬────────┬─────────────────┬───────────────────┬────────────┬──────────────┬───────────────┬──────────────┐\n",
      "│                Method │ sLimit │ iLimit │ Mean Iterations │ Median Iterations │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │\n",
      "├───────────────────────┼────────┼────────┼─────────────────┼───────────────────┼────────────┼──────────────┼───────────────┼──────────────┤\n",
      "│       NR (pinv=False)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           21.06\u001b[0m │              16.0\u001b[0m │       1102\u001b[0m │        11.02\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│       GN (pinv=False)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           21.41\u001b[0m │              16.0\u001b[0m │       1100\u001b[0m │         11.0\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│        NR (pinv=True)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           21.15\u001b[0m │              16.0\u001b[0m │       1110\u001b[0m │         11.1\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│        GN (pinv=True)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           21.91\u001b[0m │              16.0\u001b[0m │       1073\u001b[0m │        10.73\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│ LM (Wampler λ=0.0001)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           20.41\u001b[0m │              14.0\u001b[0m │        921\u001b[0m │         9.21\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│  LM (Wampler λ=1e-06)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           30.42\u001b[0m │              17.0\u001b[0m │        535\u001b[0m │         5.35\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│       LM (Chan λ=1.0)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           16.58\u001b[0m │              14.0\u001b[0m │       1011\u001b[0m │        10.11\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│       LM (Chan λ=0.1)\u001b[0m │      1\u001b[0m │    500\u001b[0m │            9.43\u001b[0m │               9.0\u001b[0m │        961\u001b[0m │         9.61\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│ LM (Sugihara λ=0.001)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           20.54\u001b[0m │              15.0\u001b[0m │       1024\u001b[0m │        10.24\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│LM (Sugihara λ=0.0001)\u001b[0m │      1\u001b[0m │    500\u001b[0m │           17.01\u001b[0m │              14.0\u001b[0m │       1011\u001b[0m │        10.11\u001b[0m │           1.0\u001b[0m │          1.0\u001b[0m │\n",
      "│       NR (pinv=False)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           29.94\u001b[0m │              18.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.46\u001b[0m │         19.0\u001b[0m │\n",
      "│       GN (pinv=False)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           30.51\u001b[0m │              18.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.48\u001b[0m │         31.0\u001b[0m │\n",
      "│        NR (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           30.17\u001b[0m │              18.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.47\u001b[0m │         17.0\u001b[0m │\n",
      "│        GN (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           30.07\u001b[0m │              18.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.47\u001b[0m │         18.0\u001b[0m │\n",
      "│ LM (Wampler λ=0.0001)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           25.16\u001b[0m │              15.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.35\u001b[0m │         21.0\u001b[0m │\n",
      "│  LM (Wampler λ=1e-06)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           29.13\u001b[0m │              18.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.44\u001b[0m │         27.0\u001b[0m │\n",
      "│       LM (Chan λ=1.0)\u001b[0m │    100\u001b[0m │     30\u001b[0m │            22.6\u001b[0m │              15.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.25\u001b[0m │         18.0\u001b[0m │\n",
      "│       LM (Chan λ=0.1)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           15.33\u001b[0m │               9.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │           1.2\u001b[0m │         18.0\u001b[0m │\n",
      "│ LM (Sugihara λ=0.001)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           26.49\u001b[0m │              16.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.35\u001b[0m │         18.0\u001b[0m │\n",
      "│LM (Sugihara λ=0.0001)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           23.04\u001b[0m │              15.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.26\u001b[0m │         18.0\u001b[0m │\n",
      "└───────────────────────┴────────┴────────┴─────────────────┴───────────────────┴────────────┴──────────────┴───────────────┴──────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "table.print()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='rtb'></a>\n",
    "\n",
    "### 4.7 Fast IK Solvers with the Robotics Toolbox\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section we will briefly demonstrate the IK solvers implemented in the Robotics Toolbox. \n",
    "\n",
    "The Toolbox has implemented each of the above IK techniques using C extensions and wrapped by python methods. The results generated from these methods will not perfectly replicate the above results as\n",
    "\n",
    "* We can not use the same random number generator or seed\n",
    "* The C implementations use different matrix inverse and pseudo-inverse calculators\n",
    "\n",
    "These methods have some advanced parameters which we have not covered yet but will cover in Part II of the tutorial. These will be ignored here."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We make a simplified wrapper class for this experiment as we only wish to collect the data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "class RTB_IK:\n",
    "    def __init__(self, name, solve, problems=problems):\n",
    "\n",
    "        # Solver attributes\n",
    "        self.name = name\n",
    "        self.solve = solve\n",
    "\n",
    "        # Solver results\n",
    "        self.success = np.zeros(problems)\n",
    "        self.searches = np.zeros(problems)\n",
    "        self.iterations = np.zeros(problems)\n",
    "\n",
    "        # initialise with NaN\n",
    "        self.success[:] = np.nan\n",
    "        self.searches[:] = np.nan\n",
    "        self.iterations[:] = np.nan\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create the experiment parameters here"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems to solve\n",
    "problems = 10000\n",
    "\n",
    "# Cartesion DoF priority matrix\n",
    "we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_rand = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_rand[i])\n",
    "\n",
    "# Maximum iterations allowed in a search\n",
    "ilimit = 30\n",
    "\n",
    "# Maximum searches allowed per problem\n",
    "slimit = 100\n",
    "\n",
    "# Solution tolerance\n",
    "tol = 1e-6"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create a list of solvers similar to Section 4.6 but use `lambda` funtions to point to the solver we wish to use. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "rtb_solvers = [\n",
    "    RTB_IK(\n",
    "        \"Newton Raphson (pinv=False)\",\n",
    "        lambda Tep: ets.ik_NR(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=False,\n",
    "            joint_limits=False\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"Gauss Newton (pinv=False)\",\n",
    "        lambda Tep: ets.ik_GN(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=False,\n",
    "            joint_limits=False\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"Newton Raphson (pinv=True)\",\n",
    "        lambda Tep: ets.ik_NR(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=True,\n",
    "            joint_limits=False\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"Gauss Newton (pinv=True)\",\n",
    "        lambda Tep: ets.ik_GN(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            pinv=True,\n",
    "            joint_limits=False\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Wampler 1e-4\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=1e-4,\n",
    "            joint_limits=False,\n",
    "            method=\"wampler\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Wampler 1e-6\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=1e-6,\n",
    "            joint_limits=False,\n",
    "            method=\"wampler\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Chan 1.0\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=1.0,\n",
    "            joint_limits=False,\n",
    "            method=\"chan\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Chan 0.1\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            q0=None,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=0.1,\n",
    "            joint_limits=False,\n",
    "            method=\"chan\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Sugihara 0.001\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=0.001,\n",
    "            joint_limits=False,\n",
    "            method=\"sugihara\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "    RTB_IK(\n",
    "        \"LM Sugihara 0.0001\",\n",
    "        lambda Tep: ets.ik_LM(\n",
    "            Tep,\n",
    "            ilimit=ilimit,\n",
    "            slimit=slimit,\n",
    "            tol=tol,\n",
    "            mask=we,\n",
    "            k=0.0001,\n",
    "            joint_limits=False,\n",
    "            method=\"sugihara\"\n",
    "        ),\n",
    "        problems=problems,\n",
    "    ),\n",
    "]\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can run the experiment. This will be much faster than the Python implementations shown above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Prog |################################| 10000/10000\n",
      "\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Numerical Inverse Kinematics Methods Compared over 10000 problems\n",
      "\n",
      "┌────────────────────────────┬────────┬────────┬─────────────────┬───────────────────┬────────────┬──────────────┬───────────────┬──────────────┐\n",
      "│                     Method │ sLimit │ iLimit │ Mean Iterations │ Median Iterations │ Infeasible │ Infeasible % │ Mean Searches │ Max Searches │\n",
      "├────────────────────────────┼────────┼────────┼─────────────────┼───────────────────┼────────────┼──────────────┼───────────────┼──────────────┤\n",
      "│Newton Raphson (pinv=False)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           28.85\u001b[0m │              17.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.46\u001b[0m │         25.0\u001b[0m │\n",
      "│  Gauss Newton (pinv=False)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           29.37\u001b[0m │              17.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.47\u001b[0m │         24.0\u001b[0m │\n",
      "│ Newton Raphson (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │            29.3\u001b[0m │              17.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.48\u001b[0m │         26.0\u001b[0m │\n",
      "│   Gauss Newton (pinv=True)\u001b[0m │    100\u001b[0m │     30\u001b[0m │           29.32\u001b[0m │              17.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.48\u001b[0m │         22.0\u001b[0m │\n",
      "│            LM Wampler 1e-4\u001b[0m │    100\u001b[0m │     30\u001b[0m │           23.92\u001b[0m │              14.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.34\u001b[0m │         18.0\u001b[0m │\n",
      "│            LM Wampler 1e-6\u001b[0m │    100\u001b[0m │     30\u001b[0m │           28.86\u001b[0m │              17.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.46\u001b[0m │         19.0\u001b[0m │\n",
      "│                LM Chan 1.0\u001b[0m │    100\u001b[0m │     30\u001b[0m │           21.47\u001b[0m │              14.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.25\u001b[0m │         22.0\u001b[0m │\n",
      "│                LM Chan 0.1\u001b[0m │    100\u001b[0m │     30\u001b[0m │           15.12\u001b[0m │               9.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │           1.2\u001b[0m │         16.0\u001b[0m │\n",
      "│          LM Sugihara 0.001\u001b[0m │    100\u001b[0m │     30\u001b[0m │           25.95\u001b[0m │              15.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.37\u001b[0m │         18.0\u001b[0m │\n",
      "│         LM Sugihara 0.0001\u001b[0m │    100\u001b[0m │     30\u001b[0m │           22.38\u001b[0m │              14.0\u001b[0m │          0\u001b[0m │          0.0\u001b[0m │          1.28\u001b[0m │         28.0\u001b[0m │\n",
      "└────────────────────────────┴────────┴────────┴─────────────────┴───────────────────┴────────────┴──────────────┴───────────────┴──────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "with Bar(message=\"Prog\", max=problems, check_tty=False, hide_cursor=False) as bar:\n",
    "    for i, pose in enumerate(Tep):\n",
    "\n",
    "        for solver in rtb_solvers:\n",
    "            (\n",
    "                _,\n",
    "                solver.success[i],\n",
    "                solver.iterations[i],\n",
    "                solver.searches[i],\n",
    "                _,\n",
    "            ) = solver.solve(pose)\n",
    "\n",
    "        bar.next()\n",
    "    bar.finish()\n",
    "\n",
    "\n",
    "# Make a table to visualise the results\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Compared over {problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"sLimit\",\n",
    "    \"iLimit\",\n",
    "    \"Mean Iterations\",\n",
    "    \"Median Iterations\",\n",
    "    \"Infeasible\",\n",
    "    \"Infeasible %\",\n",
    "    \"Mean Searches\",\n",
    "    \"Max Searches\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for solver in rtb_solvers:\n",
    "    # print(solver.iterations)\n",
    "    table.row(\n",
    "        solver.name,\n",
    "        slimit,\n",
    "        ilimit,\n",
    "        np.round(np.nanmean(solver.iterations), 2),\n",
    "        np.nanmedian(solver.iterations),\n",
    "        np.sum(np.isnan(solver.iterations)),\n",
    "        np.round(np.sum(np.isnan(solver.iterations)) / problems * 100.0, 2),\n",
    "        np.round(np.nanmean(solver.searches), 2),\n",
    "        np.nanmax(solver.searches),\n",
    "    )\n",
    "\n",
    "table.print()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='speed'></a>\n",
    "\n",
    "### 4.8 Robotics Toolbox IK Speed Comparison\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this last section we will compare the speed of some of the Robotics Toolbox IK implementations. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems to solve\n",
    "problems = 10000\n",
    "\n",
    "# Cartesion DoF priority matrix\n",
    "we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_rand = ets.random_q(problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((problems, 4, 4))\n",
    "\n",
    "for i in range(problems):\n",
    "    Tep[i] = ets.eval(q_rand[i])\n",
    "\n",
    "# Maximum iterations allowed in a search\n",
    "ilimit = 30\n",
    "\n",
    "# Maximum searches allowed per problem\n",
    "slimit = 100\n",
    "\n",
    "# Solution tolerance\n",
    "tol = 1e-6"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the solver list, we aren't bothering with a wrapper class since we don't care about the stats, only the speed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_solvers = [\n",
    "    lambda Tep: ets.ik_NR(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=False,\n",
    "        joint_limits=False,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_GN(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=False,\n",
    "        joint_limits=False,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_NR(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=True,\n",
    "        joint_limits=False,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_GN(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        pinv=True,\n",
    "        joint_limits=False,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=1e-4,\n",
    "        joint_limits=False,\n",
    "        method=\"wampler\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=1e-6,\n",
    "        joint_limits=False,\n",
    "        method=\"wampler\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=1.0,\n",
    "        joint_limits=False,\n",
    "        method=\"chan\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        q0=None,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=0.1,\n",
    "        joint_limits=False,\n",
    "        method=\"chan\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=0.001,\n",
    "        joint_limits=False,\n",
    "        method=\"sugihara\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=we,\n",
    "        k=0.0001,\n",
    "        joint_limits=False,\n",
    "        method=\"sugihara\",\n",
    "    ),\n",
    "]\n",
    "\n",
    "speed_names = [\n",
    "    \"Newton Raphson (pinv=False)\",\n",
    "    \"Gauss Newton (pinv=False)\",\n",
    "    \"Newton Raphson (pinv=True)\",\n",
    "    \"Gauss Newton (pinv=True)\",\n",
    "    \"LM Wampler 1e-4\",\n",
    "    \"LM Wampler 1e-6\",\n",
    "    \"LM Chan 1.0\",\n",
    "    \"LM Chan 0.1\",\n",
    "    \"LM Sugihara 0.001\",\n",
    "    \"LM Sugihara 0.0001\",\n",
    "]\n",
    "\n",
    "times = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Solving with Newton Raphson (pinv=False)\n",
      "Solving with Gauss Newton (pinv=False)\n",
      "Solving with Newton Raphson (pinv=True)\n",
      "Solving with Gauss Newton (pinv=True)\n",
      "Solving with LM Wampler 1e-4\n",
      "Solving with LM Wampler 1e-6\n",
      "Solving with LM Chan 1.0\n",
      "Solving with LM Chan 0.1\n",
      "Solving with LM Sugihara 0.001\n",
      "Solving with LM Sugihara 0.0001\n",
      "\n",
      "Numerical Inverse Kinematics Methods Times Compared over 10000 problems\n",
      "\n",
      "┌────────────────────────────┬────────────────┬────────────────────────────────┐\n",
      "│                     Method │ Total Time (s) │ Average Time per Solution (μs) │\n",
      "├────────────────────────────┼────────────────┼────────────────────────────────┤\n",
      "│Newton Raphson (pinv=False)\u001b[0m │         0.4279\u001b[0m │                        42.7927\u001b[0m │\n",
      "│  Gauss Newton (pinv=False)\u001b[0m │          0.655\u001b[0m │                         65.499\u001b[0m │\n",
      "│ Newton Raphson (pinv=True)\u001b[0m │         1.4902\u001b[0m │                       149.0235\u001b[0m │\n",
      "│   Gauss Newton (pinv=True)\u001b[0m │         1.5178\u001b[0m │                       151.7847\u001b[0m │\n",
      "│            LM Wampler 1e-4\u001b[0m │         0.4097\u001b[0m │                        40.9659\u001b[0m │\n",
      "│            LM Wampler 1e-6\u001b[0m │         0.4746\u001b[0m │                        47.4595\u001b[0m │\n",
      "│                LM Chan 1.0\u001b[0m │         0.3534\u001b[0m │                        35.3359\u001b[0m │\n",
      "│                LM Chan 0.1\u001b[0m │         0.2522\u001b[0m │                        25.2209\u001b[0m │\n",
      "│          LM Sugihara 0.001\u001b[0m │         0.4253\u001b[0m │                        42.5261\u001b[0m │\n",
      "│         LM Sugihara 0.0001\u001b[0m │         0.3661\u001b[0m │                        36.6059\u001b[0m │\n",
      "└────────────────────────────┴────────────────┴────────────────────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "for name, solver in zip(speed_names, speed_solvers):\n",
    "    print(f\"Solving with {name}\")\n",
    "\n",
    "    start = time.time()\n",
    "\n",
    "    for i in range(problems):\n",
    "        solver(Tep[i])\n",
    "\n",
    "    total_time = time.time() - start\n",
    "    times.append(total_time)\n",
    "\n",
    "\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Times Compared over {problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"Total Time (s)\",\n",
    "    \"Average Time per Solution (μs)\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for name, t in zip(speed_names, times):\n",
    "    table.row(\n",
    "        name,\n",
    "        np.round(t, 4),\n",
    "        np.round((t / problems) * 1e6, 4),\n",
    "    )\n",
    "\n",
    "table.print()"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
